% File : DynamicsParameters.m % Project : AUVSimulator % % %Physical parameters of a Dynamic AUV Model % %Author : O. Doucy %Based on 6DOF of NPS Phoenix %Date September 20th 1999 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Configuration dynamicsConfiguration = ('Phoenix') % Initial Conditions % % Absolute Position % X0 = 0; % m Y0 = 0; % m Z0 = 5.; %m % Atttitude % ROLL0 = 0; %deg PITCH0 = 0; %deg HEADING0 = 0;%deg % Vehicle velocities relative to fluid in body fixed coordinate % U0 = 0; % m/s V0 = 0; % m/s W0 = 0; % m/s % Angular Velocities % P0 = 0; %deg/s Q0 = 0; %deg/s R0 = 0; %deg/s %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Weight and Inertia WEIGHT = 1934.9; % Newton */ MASS = 197.2; % Kg */ BUOYANCY = 1934.9; %Newton (in real AUV slightly buoyant) */ LENGTH = 2.225; % meters */ IXX = 3.66; % Nmsec^2 Mass Moment of Inertia x axis */ IYY = 56.94; % Nmsec^2 Mass Moment of Inertia y axis */ IZZ = 61.0; % Nmsec^2 Mass Moment of Inertia z axis */ IXY = 0.; % Nmsec^2 Mass Moment of Inertia xy axis */ IYZ = 0.; % Nmsec^2 Mass Moment of Inertia yz axis */ IXZ = 0.; % Nmsec^2 Mass Moment of Inertia xz axis */ XG = 0.003; % m xcoordinate of CG from body fixed origin */ YG = 0.00; % m y coordinate of CG from body fixed origin */ ZG = 0.0128; % m z coordinate of CG from body fixed origin */ XB = 0.003; % m xcoordinate of CB from body fixed origin */ YB = 0.00; % m y coordinate of CB from body fixed origin */ ZB = 0.0; % m z coordinate of CB from body fixed origin */ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Geometry */ XBVT = 0.432; % m xcoordinate of bow vertical thruster from body fixed origin */ XSVT = -0.432; % m xcoordinate of stern vertical thruster from body fixed origin */ XBLT = 0.585; % m xcoordinate of bow lateral thruster from body fixed origin */ XSLT = -0.585; % m xcoordinate of stern lateral thruster from body fixed origin */ YLS = -0.10; % m y coordinate of port propeller */ YRS = 0.10; % m y coordinate of starbord propeller */ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Actuators Constants */ FLSF = 130; % N port propeller force forward*/ FLSR = -130; % N port propeller force reverse*/ FRSF = 130; % N starboard propeller force forward*/ FRSR = -130; % N starboard propeller force reverse*/ FLSF = 1E10; % N port propeller force forward*/ FLSR = -1E10; % N port propeller force reverse*/ FRSF = 1E10; % N starboard propeller force forward*/ FRSR = -1E10; % N starboard propeller force reverse*/ FBLT = 22.25; % N Bow Lateral thruster force */ FSLT = 22.25; % N Stern Lateral thruster force*/ FBVT = 22.25; % N Bow Vertical thruster force*/ FSVT = 22.25; % N Stern Vertical thruster force*/ MAX_PROP_RPS = 13.33; % round per sec Max propeller speed */ %Ducted Thrusters KBLT = 0.0154; % N/V2 proportionnal coef Thrust/Applied volts */ % Model : Thrust = K * |V|*V% KSLT = 0.0154; % N/V2 proportionnal coef Thrust/Applied volts */ % Model : Thrust = K * |V|*V% KBVT = 0.0154; % N/V2 proportionnal coef Thrust/Applied volts */ % Model : Thrust = K * |V|*V% KSVT = 0.0154; % N/V2 proportionnal coef Thrust/Applied volts */ % Model : Thrust = K * |V|*V% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Propellers % Model Thrustdot = (-1/tau)( Thrust + gamma * u |n| n + beta|n||n|) % identified coefficients J.S RIEDEL 1998 %Right screw TAU_R= 3.1; % s Propeller thrust time constant GAMMA_R = -0.07; % rev-1 % BETA_R = 0.045; % m.rev-2 % Left screw TAU_L = 3.1; % s Propeller thrust time constant GAMMA_L = -0.07; % rev-1 % BETA_L = 0.045; % m.rev-2 %Model Brutzman 94 Thrust =K * n|n| - speed correction included in drag% %Right screw KRS = 0.715; % N/sec2 % Left screw % KLS = 0.715; % N/sec2 DELTABR = 0.4; % rad Bow rudder deflection */ DELTASR = 0.4; % rad Stern rudder deflection */ DELTABP = 0.4; % rad Bow plane deflection */ DELTASP = 0.4; % rad Stern plane deflection */ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Adimensional Hydrodynamics Coefficients */ % Surge */ XPP = 0.; XPR = 0.; XVP = 0.; XVV = -0.01743; XQQ = 0.; XUDOT = -0.00282; XVR = 0.; XWW = 0.; XRR=-0.00753; XWQ = 0.; XQDELTABP = 0.; XQDELTASP = 0.; XRDELTASR = 0.; XRDELTABR = 0.; XDELTARDELTAR=-0.01018; XDELTASPDELTASP=-0.01018; XDELTABPDELTABP=-0.01018; XRES = -7.589e-3; % Sway */ YPDOT = 0.; YRDOT = -0.00178; YPQ = 0.; YQR = 0.; YVDOT = -0.03430; YUP = 0.; YUR = 0.01187; YVQ = 0.; YWP = 0.; YWR = 0.; YUV = -0.107; YVW = 0.; YDELTASR = 0.01241; YDELTABR = 0.01241; % Heave */ ZQDOT = -0.00253; ZPP = 0.; ZPR = 0.; ZRR = 0.; ZWDOT =-0.03430; ZUQ = -0.07013; ZVP = 0.0; ZVR = 0.; ZUW = -0.78440; ZVV = 0.; ZDELTASP = -0.02110; ZDELTABP = -0.02110; % Roll */ KPDOT = -0.00024; KRDOT = 0.; KPQ = 0.; KQR = 0.; KVDOT =0.; KUP = -0.00540; KPP = - 0.0202; KP = -3.52E-4; KUR = 0.0; KVQ = 0.; KWP = 0.; KWR = 0.; KUV = 0.; KVW = 0.; KDELTASP = 0.0; KDELTABP = 0.0; % Pitch */ MQDOT = -0.00625; MPP = 0.; MPR = 0.; MRR = 0.; MWDOT =-0.00253; MUQ = -0.01530; MVP = 0.0; MVR = 0.; MUW = 0.05122; MVV = 0.; MDELTASP = -4.55e-3; MDELTABP = 3.42e-3; % Yaw */ NPDOT = -0.00; NRDOT = -0.00047; NPQ = 0.; NQR = 0.; NVDOT =-0.00178; NUP = 0.; NUR = -0.00390; NVQ = 0.; NWP = 0.; NWR = 0.; NVW = 0.; NRR = 0.; NUV = -0.00769; NDELTASR = -4.55e-3; NDELTABR = 3.42e-3; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Cross flow Drag Coefficients */ CDY = 0.5; CDZ = 0.6; NUMBER_SLICE = 15; SLICE_X_LOC =[-1.1125 ... -1. ... -0.9 ... -0.8 ... -0.7 ... -0.25 ... 0 ... 0.25 ... 0.7 ... 0.8 ... 0.9 ... 1. ... 1.085 ... 1.11 ... 1.1125 ]; SLICE_HEIGHT =[ 0.0 ... 0.07 ... 0.133 ... 0.19 ... 0.26 ... 0.26 ... 0.26 ... 0.26 ... 0.26 ... 0.245 ... 0.194 ... 0.143 ... 0.107 ... 0.058 ... 0. ]; SLICE_BREADTH =[0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.423 ... 0.395 ... 0.32 ... 0.243 ... 0.179 ... 0.100 ... 0.]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Dimension coefficients DIM2 = RHO * LENGTH^2 /2; DIM3 = RHO * LENGTH^3 /2; DIM4 = RHO * LENGTH^4 /2; DIM5 = RHO * LENGTH^5 /2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Dimensioned Hydrodynamic coefficients */ % surge coefficients Xpp = XPP * DIM4; Xpr = XPR * DIM4; Xvp = XVP * DIM3; Xvv = XVV * DIM2; Xqq = XQQ * DIM4; Xudot = XUDOT * DIM3; Xvr = XVR * DIM3; Xrdeltabr = XRDELTABR * DIM3; Xww = XWW * DIM2; Xrr = XRR * DIM4; Xwq = XWQ * DIM3; Xqdeltabp = XQDELTABP * DIM3; Xrdeltasr = XRDELTASR * DIM3; Xdeltardeltar = XDELTARDELTAR * DIM2; Xdeltaspdeltasp = XDELTASPDELTASP * DIM2; Xdeltabpdeltabp = XDELTABPDELTABP * DIM2; Xqdeltasp = XQDELTASP * DIM3; Xres = XRES * DIM2; Xprop = 6.98579e-6; %sway coefficients Ypdot = YPDOT * DIM4; Yrdot = YRDOT * DIM4; Ypq = YPQ * DIM4; Yqr = YQR * DIM4; Yvdot = YVDOT * DIM3; Yup = YUP * DIM3; Yur = YUR * DIM3; Yvq = YVQ * DIM3; Ywp = YWP * DIM3; Ywr = YWR * DIM3; Yuv = YUV * DIM2; Yvw = YVW * DIM2; Ydeltasr = YDELTASR * DIM2; Ydeltabr = YDELTABR * DIM2; %heave coeff Zqdot = ZQDOT * DIM4; Zpp = ZPP * DIM4; Zpr = ZPR * DIM4; Zrr = ZRR * DIM4; Zwdot = ZWDOT * DIM3; Zuq = ZUQ * DIM3; Zvp = ZVP * DIM3; Zvr = ZVR * DIM3; Zuw = ZUW * DIM2; Zvv = ZVV * DIM2; Zdeltasp = ZDELTASP * DIM2; Zdeltabp = ZDELTABP * DIM2; %roll coeff Kpdot = KPDOT * DIM5; Krdot = KRDOT * DIM5; Kpq = KPQ * DIM5; Kpp = KPP * DIM5; Kp = KP * DIM5; % to be checked adimensionalisation Kqr = KQR * DIM5; Kvdot = KVDOT * DIM4; Kup = KUP * DIM4; Kur = KUR * DIM4; Kvq = KVQ * DIM4; Kwp = KWP * DIM4; Kwr = KWR * DIM4; Kuv = KUV * DIM3; Kvw = KVW * DIM3; Kdeltabp = KDELTABP * DIM3; Kdeltasp = KDELTASP * DIM3; %pitch coeff Mqdot = MQDOT * DIM5; Mpp = MPP * DIM5; Mpr = MPR * DIM5; Mrr = MRR * DIM5; Mwdot = MWDOT * DIM4; Muq = MUQ * DIM4; Mvp = MVP * DIM4; Mvr = MVR * DIM4; Muw = MUW * DIM3; Mvv = MVV * DIM3; Mdeltasp = MDELTASP * DIM3; Mdeltabp = MDELTABP * DIM3; %yaw coeff Npdot = NPDOT * DIM5; Nrdot = NRDOT * DIM5; Npq = NPQ * DIM5; Nqr = NQR * DIM5; Nvdot = NVDOT * DIM4; Nup = NUP * DIM4; Nur = NUR * DIM4; Nvq = NVQ * DIM4; Nwp = NWP * DIM4; Nwr = NWR * DIM4; Nuv = NUV * DIM3; Nvw = NVW * DIM3; Ndeltasr = NDELTASR * DIM3; Ndeltabr = NDELTABR * DIM3; Nrr = NRR * DIM5 ; MASS_MATRIX = [ MASS 0 0 0 0 0; 0 MASS 0 0 0 0; 0 0 MASS 0 0 0; 0 0 0 IXX IXY IXZ; 0 0 0 IXY IYY IYZ; 0 0 0 IXZ IYZ IZZ]; MASS_ADDED_MATRIX = [ MASS-Xudot 0 0 0 MASS*ZG -MASS*YG; 0 MASS-Yvdot 0 -(MASS*ZG+Ypdot) 0 MASS*XG-Yrdot; 0 0 MASS-Zwdot MASS*YG -(MASS*XG+Zqdot) 0; 0 -(MASS*ZG+Kvdot) MASS*YG IXX-Kpdot -(IXY) -(IXZ+Krdot); MASS*ZG 0 -(MASS*XG+Mwdot) -(IXY) IYY-Mqdot -(IYZ); -(MASS*YG) MASS*XG-Nvdot 0 -(IXZ+Npdot) -(IYZ) IZZ-Nrdot]; INVERSE_ADDED_MASS = (inv(MASS_ADDED_MATRIX)+ (inv(MASS_ADDED_MATRIX))')/2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Simulink model parameters */ PHYSICAL_CONSTANTS = [RHO GRAVITY PI]; PHYSICAL_CONSTANTS = [length(PHYSICAL_CONSTANTS) PHYSICAL_CONSTANTS ]; GEOMETRY = [LENGTH WEIGHT BUOYANCY XG YG ZG XB YB ZB XBVT XSVT XBLT XSLT YLS YRS ]; GEOMETRY = [ length(GEOMETRY) GEOMETRY]; INERTIA = [MASS_MATRIX INVERSE_ADDED_MASS]; ACTUATORS_CONSTANTS = [ FLSF FLSR FRSF FRSR FBLT FSLT FBVT FSVT ... KBLT KSLT KBVT KSVT... TAU_R GAMMA_R BETA_R TAU_L GAMMA_L BETA_L ... KRS KLS DELTABR DELTASR DELTABP DELTASP ]; ACTUATORS_CONSTANTS = [length(ACTUATORS_CONSTANTS) ACTUATORS_CONSTANTS]; CROSS_FLOW_REAL = [ NUMBER_SLICE CDY CDZ ]; CROSS_FLOW_GEOMETRY = [SLICE_X_LOC SLICE_HEIGHT SLICE_BREADTH]; HYDRODYNAMICS_SURGE = [Xpp Xpr Xvp Xvv Xqq Xudot Xvr Xww Xrr Xwq Xqdeltasp ... Xqdeltabp Xrdeltasr Xrdeltabr Xdeltardeltar Xdeltaspdeltasp Xdeltabpdeltabp ... Xres Xprop ]; HYDRODYNAMICS_SURGE = [length(HYDRODYNAMICS_SURGE) HYDRODYNAMICS_SURGE]; HYDRODYNAMICS_SWAY = [Ypdot Yrdot Ypq Yqr Yvdot Yup Yur Yvq Ywp Ywr Yuv Yvw ... Ydeltasr Ydeltabr ]; HYDRODYNAMICS_SWAY = [length(HYDRODYNAMICS_SWAY) HYDRODYNAMICS_SWAY]; HYDRODYNAMICS_HEAVE = [Zqdot Zpp Zpr Zrr Zwdot Zuq Zvp Zvr Zuw Zvv ... Zdeltasp Zdeltabp ]; HYDRODYNAMICS_HEAVE = [length(HYDRODYNAMICS_HEAVE) HYDRODYNAMICS_HEAVE] ; HYDRODYNAMICS_ROLL = [Kpdot Krdot Kpq Kqr Kpp Kp Kvdot Kup Kur Kvq Kwp Kwr Kuv Kvw ... Kdeltabp Kdeltasp ]; HYDRODYNAMICS_ROLL = [length(HYDRODYNAMICS_ROLL) HYDRODYNAMICS_ROLL]; HYDRODYNAMICS_PITCH = [ Mqdot Mpp Mpr Mrr Mwdot Muq Mvp Mvr Muw Mvv ... Mdeltasp Mdeltabp ]; HYDRODYNAMICS_PITCH = [length(HYDRODYNAMICS_PITCH) HYDRODYNAMICS_PITCH]; HYDRODYNAMICS_YAW = [ Npdot Nrdot Npq Nqr Nvdot Nup Nur Nvq Nwp Nwr Nuv Nvw Nrr... Ndeltasr Ndeltabr ]; HYDRODYNAMICS_YAW = [length(HYDRODYNAMICS_YAW) HYDRODYNAMICS_YAW]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Initial Conditions POS_0 = [ X0 Y0 Z0 ROLL0 * pi/180 PITCH0 * pi/180 HEADING0 * pi/180]; BFV_0 = [ U0 V0 W0 P0 * pi/180 Q0 * pi/180 R0 * pi/180];